using Godot;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
public partial class MyMeshInstance3d : MeshInstance3D
{
    List<Vector3> verts = [];
    List<Vector3> positions = [];
    List<Vector2> uvs = [];
    List<Vector3> normals = [];
    List<Vector3> forces = [];
    List<int> indices = [];
    List<Vector3> velocitys = [];
    private int _rings = 25;
    private int _radialSegments = 25;
    private float _radius = 1;
    Godot.Collections.Array surfaceArray = [];
    Dictionary<ulong, float> distanceConstrains = [];
    List<int> fixedConstrains = [];
    List<(int index, Vector3 qc, Vector3 nc)> collisionConstrains = [];
    public int SphereLine(Vector3 p1, Vector3 p2, Vector3 sc, double r, out double? mu1, out double? mu2, out Vector3? o1, out Vector3? o2)
    {
        double a, b, c;
        double bb4ac;
        Vector3 dp;
        dp.X = p2.X - p1.X;
        dp.Y = p2.Y - p1.Y;
        dp.Z = p2.Z - p1.Z;
        a = dp.X * dp.X + dp.Y * dp.Y + dp.Z * dp.Z;
        b = 2 * (dp.X * (p1.X - sc.X) + dp.Y * (p1.Y - sc.Y) + dp.Z * (p1.Z - sc.Z));
        c = sc.X * sc.X + sc.Y * sc.Y + sc.Z * sc.Z;
        c += p1.X * p1.X + p1.Y * p1.Y + p1.Z * p1.Z;
        c -= 2 * (sc.X * p1.X + sc.Y * p1.Y + sc.Z * p1.Z);
        c -= r * r;
        bb4ac = b * b - 4 * a * c;
        if (bb4ac < 0)
        {
            o1 = null;
            o2 = null;
            mu1 = null;
            mu2 = null;
            return 0;
        }
        else if (Math.Abs(a) == 0)
        {
            mu1 = -b / c;
            mu2 = null;
            o1 = p1 + (p2 - p1) * (float)mu1;
            o2 = null;
            return 1;
        }
        else
        {
            mu1 = (-b + Math.Sqrt(bb4ac)) / (2 * a);
            mu2 = (-b - Math.Sqrt(bb4ac)) / (2 * a);
            // 确保交点mu1在p1和p2之间
            if (mu2 >= 0 && mu2 <= 1) (mu1, mu2) = (mu2, mu1);
            o1 = p1 + (p2 - p1) * (float)mu1;
            o2 = p1 + (p2 - p1) * (float)mu2;
            return 2;
        }

    }

    public override void _Process(double delta)
    {
        base._Process(delta);
        PositionBasedDynamics(0.02);
        //GD.Print(velocitys[0]);
        surfaceArray[(int)Mesh.ArrayType.Vertex] = verts.ToArray();
        var arrMesh = Mesh as ArrayMesh;
        //// No blendshapes, lods, or compression used.
        arrMesh?.ClearSurfaces();
        arrMesh?.AddSurfaceFromArrays(Mesh.PrimitiveType.Triangles, surfaceArray);
    }

    private void PositionBasedDynamics(double delta)
    {
        //  negetive Y
        var g = 10.0;
        PhysicsDirectSpaceState3D spaceState = GetWorld3D().DirectSpaceState;
        var center = Vector3.Zero;
        // 构造中心力
        for (int i = 0; i < verts.Count; i++)
        {
            center += verts[i] / verts.Count;
        }
        // 初始化位置和速度 已在节点初始化时完成
        // 以下开始循环
        //      更新节点速度  vi ← vi +Δt * wi * fext(xi)
        //      速度阻尼
        //      在无约束的情况下更新粒子位置：pi ← xi +Δt * vi
        //      碰撞约束：xi → pi
        //      迭代速度投影
        //      更新粒子速度和位置：vi ← (pi −xi)/Δt, xi ← pi
        var mass = 1;
        var wi = 1 / mass;
        var f_gravity = mass * new Vector3(0, -1, 0) * (float)g;
        collisionConstrains.Clear();
        for (var i = 0; i < verts.Count; i++)
        {
            //a = (verts[i] - center).Normalized() * 1.01f;
            //固定约束，不受外力影响
            var fext = f_gravity + forces[i];
            if (fixedConstrains.Contains(i))
                fext = Vector3.Zero;
            var damping = 0.99f;
            //半隐式积分
            if (wi != 0)
            {
                // 更新节点速度
                velocitys[i] = velocitys[i] + (float)delta * wi * fext;
                //TODO 速度阻尼
                velocitys[i] *= damping;
                // 在无约束的情况下更新粒子位置
                positions[i] = verts[i] + velocitys[i] * (float)delta;
            }

            //碰撞检测
            //在Godot的MeshInstance3D里，Mesh的位置是相对于MeshInstance3D的位置的。
            //但是当我们使用PhysicsDirectSpaceState3D的IntersectRay方法时，
            //我们需要传入的是世界坐标系下的位置。
            //所以我们需要将Mesh的位置和顶点的位置相加，得到世界坐标系下的位置。
            //在构造Mesh时，使用的是Mesh的局部坐标系。
            //在更新Mesh位置时，可以认为Mesh具有平移不变性，在世界和局部坐标系下进行位置操作是一样的。
            //另外，MeshInstance3D放到场景中时，ArrayMesh必须放到子节点里，否则位置不正确，Z方向在不同编译模式下
            //具有不同的值，不知道为什么。
            var xi = verts[i] + GlobalPosition;
            var pi = positions[i] + GlobalPosition;
            //var query = PhysicsRayQueryParameters3D.Create(xi, pi);
            //query.CollideWithAreas = true;
            //query.Exclude = [Mesh.GetRid()];
            //var result = spaceState.IntersectRay(query);
            //if (result.Count > 0)
            //{
            //    var qc = ((Vector3)result["position"]);
            //    var nc = ((Vector3)result["normal"]);
            //    var Cp = (pi - qc).Dot(nc);
            //    if (Cp < 0)
            //    {
            //        collisionConstrains.Add((i, qc, nc));
            //    }
            //}

            var sphere_center = new Vector3(-40, 10, 26);
            var radius = 10;
            var pi2c = pi - sphere_center;
            var xi2c = xi - sphere_center;
            // xi在球内，pi在球外，xi->pi与球相交
            if (pi2c.Length() <= radius && xi2c.Length() >= radius)
            {
                SphereLine(xi, pi, sphere_center, radius, out var _, out var _, out var o1, out var _);
                var qc = (Vector3)o1;
                var nc = (qc - sphere_center).Normalized();
                collisionConstrains.Add((i, qc, nc));
            }
            // xi->pi在球内
            else if (pi2c.Length() <= radius && xi2c.Length() <= radius)
            {
                var qs = sphere_center + pi2c.Normalized() * radius;
                var ns = (qs - sphere_center).Normalized();
                collisionConstrains.Add((i, qs, ns));
            }
        }
        // 迭代位置投影
        ProjectContrains();
        // 更新粒子速度和位置
        for (var i = 0; i < verts.Count; i++)
        {
            velocitys[i] = (positions[i] - verts[i]) / (float)delta;
            verts[i] = positions[i];
        }
    }

    private void ProjectContrains()
    {
        bool satisfy;
        int iteration = 0;
        var EPS = 0.1;
        do
        {
            iteration++;
            satisfy = true;
            foreach (var item in distanceConstrains)
            {
                var index1 = (int)(item.Key >> 32);
                var index2 = (int)(item.Key & 0xffffffff);
                var p1 = positions[index1];
                var p2 = positions[index2];
                var w1 = 1f;
                var w2 = 1f;
                var d = item.Value;
                // Distance Contrains
                var delta_p1 = -(w1 / (w1 + w2)) * (p1.DistanceTo(p2) - d) * (p1 - p2).Normalized();
                var delta_p2 = +(w2 / (w1 + w2)) * (p1.DistanceTo(p2) - d) * (p1 - p2).Normalized();
                //  位置约束
                if (fixedConstrains.Contains(index1)) delta_p1 = Vector3.Zero;
                if (fixedConstrains.Contains(index2)) delta_p2 = Vector3.Zero;
                positions[index1] = p1 + delta_p1;
                positions[index2] = p2 + delta_p2;
                if (delta_p1.Length() > EPS || delta_p1.Length() > EPS)
                {
                    satisfy = false;
                }
            }
            foreach (var item in collisionConstrains)
            {
                var index = item.index;
                var qc = item.qc;
                var nc = item.nc;
                var pi = positions[index] + GlobalPosition;
                var Cp = (pi - qc).Dot(nc);
                if (Cp > 0) continue;
                else satisfy = false;
                var GradCp = nc;
                var delta_p = -(Cp * GradCp / GradCp.LengthSquared()) + (float)EPS * GradCp;
                if (delta_p.Length() > EPS)
                {
                    satisfy = false;
                }
                positions[index] += delta_p;
            }
            GD.Print(iteration);
            if (iteration > 20) satisfy = true;

        } while (!satisfy);
    }

    public override void _Ready()
    {
        try
        {
            // Insert setting up the surface array and lists here.
            surfaceArray.Resize((int)Mesh.ArrayType.Max);
            // ConstructFromFile("Sphere");
            // ConstructFromFile("Box.inp");
            ConstructFromFile("Plane");

            // ConstructFromFile("ZZero");
            //GenerateSphere();
            // triangle Contrains = distance Contrains   
            for (int i = 0; i < indices.Count; i += 3)
            {
                var index1 = (uint)indices[i];
                var index2 = (uint)indices[i + 1];
                var index3 = (uint)indices[i + 2];

                var indexA1 = (ulong)index1 << 32 | index2;
                var indexA2 = (ulong)index2 << 32 | index1;
                if (!distanceConstrains.ContainsKey(indexA2)) distanceConstrains[indexA1] = verts[(int)index1].DistanceTo(verts[(int)index2]);


                var indexB1 = (ulong)index2 << 32 | index3;
                var indexB2 = (ulong)index3 << 32 | index2;
                if (!distanceConstrains.ContainsKey(indexB2)) distanceConstrains[indexB1] = verts[(int)index2].DistanceTo(verts[(int)index3]);

                var indexC1 = (ulong)index3 << 32 | index1;
                var indexC2 = (ulong)index1 << 32 | index3;
                if (!distanceConstrains.ContainsKey(indexC2)) distanceConstrains[indexC1] = verts[(int)index3].DistanceTo(verts[(int)index1]);
            }
            // Insert committing to the ArrayMesh here.
            surfaceArray[(int)Mesh.ArrayType.Vertex] = verts.ToArray();
            surfaceArray[(int)Mesh.ArrayType.TexUV] = uvs.ToArray();
            surfaceArray[(int)Mesh.ArrayType.Normal] = normals.ToArray();
            surfaceArray[(int)Mesh.ArrayType.Index] = indices.ToArray();

            var arrMesh = Mesh as ArrayMesh;
            // No blendshapes, lods, or compression used.
            arrMesh?.AddSurfaceFromArrays(Mesh.PrimitiveType.Triangles, surfaceArray);
        }
        catch (System.Exception e)
        {
            GD.Print(e.Message);
        }

    }

    private void GenerateSphere()
    {
        var thisRow = 0;
        var prevRow = 0;
        var point = 0;

        // Loop over rings.
        for (var i = 0; i < _rings + 1; i++)
        {
            var v = ((float)i) / _rings;
            // v is the vertical position. v:0->pi 由上到下 与Y轴夹角
            // u is the horizontal position. u:0->2pi 由左到右
            var w = Mathf.Sin(Mathf.Pi * v);//w:在水平面上的投影
            var y = Mathf.Cos(Mathf.Pi * v);// y:在垂直方向上的投影  Y坐标：垂直方向

            // Loop over segments in ring.
            for (var j = 0; j < _radialSegments + 1; j++)
            {
                var u = ((float)j) / _radialSegments;// u：与z轴的夹角
                var x = Mathf.Sin(u * Mathf.Pi * 2);
                var z = Mathf.Cos(u * Mathf.Pi * 2);
                var t = Time.GetTicksMsec();
                _radius = 2 + Mathf.Sin(t / 1000.0f) * 0.5f;
                var vert = new Vector3(x * _radius * w, y * _radius, z * _radius * w);
                verts.Add(vert);
                positions.Add(new Vector3(0, 0, 0));
                velocitys.Add(new Vector3(0, 0, 0));
                normals.Add(vert.Normalized());
                uvs.Add(new Vector2(u, v));
                point += 1;
                if (i > _rings / 2 && j > _radialSegments / 2)
                {
                    velocitys[^1] = new Vector3(0, 0.0f, 0);
                }
                // Create triangles in ring using indices.
                if (i > 0 && j > 0)
                {
                    indices.Add(prevRow + j - 1);
                    indices.Add(prevRow + j);
                    indices.Add(thisRow + j - 1);

                    indices.Add(prevRow + j);
                    indices.Add(thisRow + j);
                    indices.Add(thisRow + j - 1);

                    var index1 = (uint)(prevRow + j - 1);
                    var index2 = (uint)(prevRow + j);
                    var index3 = (uint)(thisRow + j - 1);

                    var index4 = (uint)(prevRow + j);
                    var index5 = (uint)(thisRow + j);
                    var index6 = (uint)(thisRow + j - 1);

                    var indexA1 = (ulong)index1 << 32 | index2;
                    var indexA2 = (ulong)index2 << 32 | index1;
                    distanceConstrains[indexA1] = verts[(int)index1].DistanceTo(verts[(int)index2]);
                    distanceConstrains[indexA2] = distanceConstrains[indexA1];

                    var indexB1 = (ulong)index2 << 32 | index3;
                    var indexB2 = (ulong)index3 << 32 | index2;
                    distanceConstrains[indexB1] = verts[(int)index2].DistanceTo(verts[(int)index3]);
                    distanceConstrains[indexB2] = distanceConstrains[indexB1];

                    var indexC1 = (ulong)index3 << 32 | index1;
                    var indexC2 = (ulong)index1 << 32 | index3;
                    distanceConstrains[indexC1] = verts[(int)index3].DistanceTo(verts[(int)index1]);
                    distanceConstrains[indexC2] = distanceConstrains[indexC1];

                    var indexD1 = (ulong)index4 << 32 | index5;
                    var indexD2 = (ulong)index5 << 32 | index4;
                    distanceConstrains[indexD1] = verts[(int)index4].DistanceTo(verts[(int)index5]);
                    distanceConstrains[indexD2] = distanceConstrains[indexD1];

                    var indexE1 = (ulong)index5 << 32 | index6;
                    var indexE2 = (ulong)index6 << 32 | index5;
                    distanceConstrains[indexE1] = verts[(int)index5].DistanceTo(verts[(int)index6]);
                    distanceConstrains[indexE2] = distanceConstrains[indexE1];

                    var indexF1 = (ulong)index6 << 32 | index4;
                    var indexF2 = (ulong)index4 << 32 | index6;
                    distanceConstrains[indexF1] = verts[(int)index6].DistanceTo(verts[(int)index4]);
                    distanceConstrains[indexF2] = distanceConstrains[indexF1];
                }
            }
            prevRow = thisRow;
            thisRow = point;
        }
    }
    public void ConstructFromFile(string fileName)
    {
        try
        {
            var lines_raw = File.ReadAllLines(fileName).Select(i => i.Trim());
            var lines = lines_raw.Where(i => !i.StartsWith("**"));
            IEnumerable<(string header, int index)> headers = lines.Select((line, index) => (line, index)).Where(v => v.line.StartsWith("*")).Select(v => (v.line[1..], v.index));
            headers = headers.Append(("end", -1));
            var indexs = headers.Select(i => i.index);
            IEnumerable<(string header, int start, int end)> headers_info = headers.ToArray()[..^1].Zip(indexs.Skip(1)).Select(x => (x.First.header, x.First.index + 1, x.Second));
            // 序号从1开始
            foreach (var (header, start, end) in headers_info)
            {
                if (header.StartsWith("node", System.StringComparison.CurrentCultureIgnoreCase))
                {
                    foreach (var line in lines.ToList()[start..end])
                    {
                        var items = line.Split(",", false).Select(static i => double.Parse(s: i)).ToList();
                        //Mesh放到了子节点里
                        var relativePosition = new Vector3((float)items[1], (float)items[2], (float)items[3]);
                        var v = relativePosition;
                        verts.Add(v);
                        positions.Add(new Vector3(0, 0, 0));
                        forces.Add(new Vector3(0, 0, 0));
                        velocitys.Add(new Vector3(0, 0, 0));
                        normals.Add(verts[^1].Normalized());
                        uvs.Add(new Vector2(0, 0));
                        if (verts[^1][0] == 0) //构造 约束 x=0处 位移为0
                        {
                            //fixedConstrains.Add(verts.Count - 1);
                        }
                    }
                }
            }

            foreach (var h in headers_info)
            {
                var name = h.header.ToLower();
                if (name.Contains("type=CPS3", System.StringComparison.CurrentCultureIgnoreCase))
                {
                    var slice = h.end == -1 ? lines.ToList()[h.start..] : lines.ToList()[h.start..h.end];
                    var point_indexs = slice.Select(x => x.Split(",", false).Skip(1).Select(x => int.Parse(x.Trim()) - 1).ToArray()).ToList();
                    Name += name + "_";
                    indices = [.. indices, .. point_indexs.SelectMany(x => x).ToList()];
                }
                if (name.Contains("type=C3D4", System.StringComparison.CurrentCultureIgnoreCase))
                {

                }

            }
        }
        catch (System.Exception e)
        {
            GD.PrintErr(e.Message);
        }

    }

}
